/**
 * @file   qsort.h
 * @author karpar Hu <huzhili@gmail.com>
 * @date   Tue Apr 28 20:48:51 2009
 * 
 * @brief  Qsort routine from Bently & McIlaroy's "Engineer a Sort Function"
 * 
 * 
 */

#ifndef _QSORT_H_
#define _QSORT_H_

#include <stdlib.h>

typedef int cmp_t(const void *, const void *);
static inline char *med3(char *, char *, char *, cmp_t *,void *) __attribute__((always_inline));
static inline void swapfunc(char *, char *, int, int) __attribute__((always_inline));

#define min(a, b) (a) < (b) ? (a) : (b)

#define swapcode(TYPE, parami, paramj, n) {     \
    long i = (n) / sizeof(TYPE);                \
    TYPE *pi = (TYPE *) (parami);               \
    TYPE *pj = (TYPE *) (paramj);               \
    do {                                        \
      TYPE t = *pi;                             \
      *pi++ = *pj;                              \
      *pj++ = t;                                \
    } while (--i > 0);                          \
  }  

/* initialize swap type: 1: multiwords, 0:single word, 2:single byte  */
#define SWAPINIT(a, es) swaptype = ((char *)a - (char *)0) % sizeof(long) || \
    es % sizeof(long) ? 2 : es == sizeof(long) ? 0 : 1;

static inline void
swapfunc(char *a, char *b, int n, int swaptype)
{
  if (swaptype <= 1)            /* word or words */
    swapcode(long, a, b, n)
  else
    swapcode(char, a, b, n)     /* char */
}

#define swap(a, b)                              \
  if (swaptype == 0) {                          \
    long t = *(long *)a;                        \
    *(long *)a = *(long *)b;                    \
    *(long *)b = t;                             \
  } else                                        \
    swapfunc(a, b, es, swaptype)                \

#define vecswap(a, b, n) if ((n) > 0) swapfunc(a, b, n, swaptype)

static inline char *
med3(char *a, char *b, char *c, cmp_t *cmp)
{
  return (cmp(a, b) < 0 ?
          (cmp(b, c) < 0 ? b : cmp(a, c) < 0 ? c : a)
          : (cmp(b, c) > 0 ? b : cmp(a, c) < 0 ? a : c));
}

void
zqsort(void *a, size_t n, size_t es, cmp_t *cmp)
{
  char *pa, *pb, *pc, *pd, *pl, *pm, *pn;
  int d, s, swaptype, swap_cnt;
  
loop: SWAPINIT(a, es);
  swap_cnt = 0;
  if (n < 7) {                  /* insert sort */
    for (pm = (char *)a + es; pm < (char *)a + n * es; pm += es) {
      for (pl = pm; pl > (char *)a && cmp(pl - es, pl) > 0; pl -= es) {
        swap(pl, (pl - es));
      }
    }
    return;
  }
  pm = (char *)a + (n / 2) * es;
  if (n > 7) {
    pl = (char *)a;
    pn = (char *)a + (n-1) * es;
    if (n > 40) {
      d = (n / 8) * es;         /* pseudo ninth */
      pl = med3(pl, pl + d, pl + 2 * d, cmp);
      pm = med3(pm-d, pm, pm + d, cmp);
      pn = med3(pn-2*d, pn - d, pn, cmp);
    }
    pm = med3(pl, pm, pn, cmp);
  }
  swap((char *)a, pm);                  /* put median in front */
  pa = pb = (char *)a + es;
  pc = pd = (char *)a + (n-1) * es;
  for (;;) {
    while (pb <= pc && (s = cmp(pb, a)) <= 0) {
      if ( s == 0 ) {
        swap_cnt = 1;
        swap(pa, pb);
        pa += es;
      }
      pb += es;
    }
    while (pb <= pc && (s = cmp(pc, a)) >= 0) {
      if ( s == 0 ) {
        swap_cnt = 1;
        swap(pc, pd);
        pd -= es;
      }
      pc -= es;
    }
    if ( pb > pc )
      break;
    swap(pb, pc);
    swap_cnt = 1;
    pb += es;
    pc -= es;
  }
  if (swap_cnt == 0) {          /* Switch to insertion sort */
    for (pm = (char *)a + es; pm < (char *)a + n * es; pm += es) {
      for (pl = pm; pl > (char *)a && cmp(pl - es, pl) > 0; pl -= es) {
        swap((pl-es), pl);
      }
    }
    return;
  }
  
  pn = (char *)a + n * es;
  s = min(pa - (char *)a, pb - pa);
  vecswap((char *)a, pb - s, s);
  s = min(pd - pc, pn - pd - es);
  vecswap(pb, pn - s, s);
  if ((s = pb - pa) > es)  
    qsort(a, s / es, es, cmp);
  if ((s = pd - pc) > es) {
    a = pn - s; /* Iterate rather than recurse to save stack sapace */
    n = s / es;
    goto loop;
  }
}

#endif /* _QSORT_H_ */
